An Efficient Closed-Form Solution to Probabilistic 6D Visual Odometry for a Stereo Camera

Estimating the ego-motion of a mobile robot has been traditionally achieved by means of encoder-based odometry. However, this method presents several drawbacks, such as the existence of accumulative drifts, its sensibility to slippage, and its limitation to planar environments. In this work we present an alternative method for estimating the incremental change in the robot pose from images taken by a stereo camera. In contrast to most previous approaches for 6D visual odometry, based on iterative, approximate methods, we propose here to employ an optimal closed-form solution for estimating the incremental change in the pose. We also derive the expression for the covariance associated to this estimation, which enables the integration of our approach into visionbased SLAM frameworks. Additionally, our proposal combines highlydistinctive SIFT descriptors with the fast KLT feature tracker, thus achieving both robust and efficient execution in real-time. To validate our research we provide experimental results for a real robot.